#ifndef IIR_FILTER_H
#define IIR_FILTER_H

#include <Eigen/Dense>

namespace ctrl{

// signal input, output, filter coeficient all in double type
class IIR_filt
{
private:
    double M;                ///< Nr of MA filter taps
    double N;                ///< Nr of AR filter taps
    Eigen::Index b_cur_p;          ///< Pointer to current sample in MA buffer
    Eigen::Index a_cur_p;          ///< Pointer to current sample in AR buffer
    
    Eigen::VectorXd b;      ///< MA Filter coefficients
    Eigen::VectorXd a;      ///< AR Filter coefficients
    Eigen::VectorXd b_buf;  ///< MA Signal buffer
    Eigen::VectorXd a_buf;  ///< AR Signal buffer
public:

    IIR_filt(){}
    ~IIR_filt(){}

    void clear(void)
    {
        b_buf.setZero();
        a_buf.setZero();
        b_cur_p = 0;
        a_cur_p = 0;
    }

    /**
   * @brief Sets coefficients in IIR filter.
   * @param _b Filter coefficients \f$ [b_0 ..b_M] \f$
   * @param _a Filter coefficients \f$ [a_0 ..a_N] \f$
   *
   */
    void set_coeffs(const Eigen::VectorXd &_b,const Eigen::VectorXd &_a)
    {
        M = _b.size();
        N = _a.size();
        b_buf.resize(M);
        a_buf.resize(N);
        this->clear();
        b = _b/_a[0];
        a = _a/_a[0];
    }
    void update_coeffs(const Eigen::VectorXd &_b,const Eigen::VectorXd &_a)
    {
        b = _b/_a[0];
        a = _a/_a[0];
    }

    /**
   * @brief Filter operator.
   * @param _b Filtered output
   * @param _a Input sample
   *
   */
    double operator()(const double & in)
    {
        double out=0;
        Eigen::Index p = 0;

        b_buf(b_cur_p) = in;                
        for(Eigen::Index m = b_cur_p; m < M; m++)
            out += b(p++)*b_buf(m);         
        for(Eigen::Index m = 0; m < b_cur_p; m++)
            out += b(p++)*b_buf(m);         

        if(b_cur_p == 0)
            b_cur_p = M-1;
        else
            b_cur_p--;

        p=1;
        for(Eigen::Index n = a_cur_p+1; n < N; n++)
            out -= a(p++)*a_buf(n);         
        for(Eigen::Index n = 0; n < a_cur_p; n++)
            out -= a(p++)*a_buf(n);         

        a_buf(a_cur_p) = out;		        

        if(a_cur_p == 0)
            a_cur_p = N-1;
        else
            a_cur_p--;

        return out;
    }

};

}
#endif